#include "Stepper_Motor.h"
//uart6

volatile uint8_t request1[] = {0x7b,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7a,0x7d};
volatile uint8_t request0[] = {0x7b,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7b,0x7d};
Stepper_Motor StepMotor0 = {
    .ID = 0x00,
    .speed = 0.0f,
    .angle = 0.0f,
    .status = 0
};
Stepper_Motor StepMotor1 = {
    .ID = 0x01,
    .speed = 0.0f,
    .angle = 0.0f,
    .status = 0
};
uint16_t Stepper_pos[4][2] = {0}; // 用于存储发送数据的缓冲区
extern UART_HandleTypeDef huart6;
volatile my_uart_struct stepper_uart6 = {
    .receivenum = 0,
    .receivedata = {0},
    .bufftemp = {0}
};

void Stepper1_SendRequest(void)
{ 

    HAL_UART_Transmit_DMA(&huart6, (const uint8_t*)request1, 11);
	//HAL_UART_Receive_DMA(&huart6, (uint8_t *)stepper_uart6.bufftemp, 9); // 使用DMA接收步进电机数据
    HAL_UART_Receive_DMA(&huart6, (uint8_t *)stepper_uart6.bufftemp, 9); // 使用阻塞接收步进电机数据

}

void Stepper0_SendRequest(void)
{

    HAL_UART_Transmit_DMA(&huart6, (const uint8_t*)request0, 11);
	HAL_UART_Receive_DMA(&huart6, (uint8_t *)stepper_uart6.bufftemp, 9); // 使用阻塞接收步进电机数据
 
}

void stepper_test_proc(void)
{
    // static uint8_t now = 0;
    // if (now == 0)
    // {
    //     addx();
    //     now = 1;

    // }
    // else if (now == 1)
    // {
    //     deductx();
    //     now = 0;
    // }
    // else if (now == 2)
    // {
    //     addy();
    //     now = 3;
    // }
    // else if (now == 3)
    // {
    //     deducty();
    //     now = 2;
    // }
}
/* 
第一个字节：电机的地址，默认为01；
第二个字节：在位置控制模式下，到达目标位置则发送1，还未到达发送0；
其实模式默认发送0；
第三个字节：速度的高八位；
第四个字节：速度的低八位；
第五个字节：角度的高16位中的高8位；
第六个字节：角度的高16位中的低8位；
第七个字节：角度的低16位中的高8位；
第八个字节：角度的低16位中的低8位;
第九个字节：RCC校验位；
如接收到的数据为：0100006400000e107B，显示的内容则为：目前电
机转速为10rad/s转动到了360度，还没有到达目标位置。如果接收数据为：0101
 00 00 00 00 8c a0 2C，显示的内容为：电机已经转动角度达到3600度位置。
 */




void Stepper_Uart_Receive(void)   // 中断回调
{
    uint8_t* rx = stepper_uart6.receivedata;

    if(rx[0] == 0x01 || rx[0] == 0x00)
    {
        uint8_t addr = rx[0];
        uint8_t arrived = rx[1];

        uint16_t speed_raw = (rx[2] << 8) | rx[3];
        float speed = speed_raw / 10.0f; // 单位 rad/s

        uint32_t angle_raw = (rx[4] << 24) | (rx[5] << 16) | (rx[6] << 8) | rx[7];
        float angle = angle_raw / 10.0f; // 单位 度
        if (addr == 0x01) {
            StepMotor1.speed = speed;
            StepMotor1.angle = angle;
            StepMotor1.status = arrived;
        } else if (addr == 0x00) {
            StepMotor0.speed = speed;
            StepMotor0.angle = angle;
            StepMotor0.status = arrived;
        }
        // printf("Stepper Motor%d Status:\n", addr);
        // printf("Speed: %.2f rad/s\n", speed);
        // printf("Angle: %.2f degrees\n", angle);
        // printf("Arrived: %d\n", arrived);
    }
}
void Stepper_Motor_Control(uint8_t ID, StepMotor_Mode mode,uint8_t direction, float speed, float angle)
{
    uint8_t data[11] = {0};
    uint8_t protocol_mode = mode + 1; // 枚举与协议值偏移1

    // 1. 帧头
    data[0] = 0x7B;
    // 2. 设备地址
    data[1] = ID;
    // 3. 控制模式
    data[2] = protocol_mode;
    if (direction == 0) {
        data[3] = 0x00; // 逆时针
    } else {
        data[3] = 0x01; // 顺时针
    }
    // 5. 细分值，写死32细分
    data[4] = 0x20;

    // 6-7. 角度或电流 (位置5-6放角度！)
    uint16_t angle_or_current = 0;
    if (protocol_mode == 0x02 || protocol_mode == 0x04) { // 位置或绝对角度
        angle_or_current = (uint16_t)(angle * 10.0f); // 角度乘以10
    }
    data[5] = (angle_or_current >> 8) & 0xFF;  // 角度高字节
    data[6] = angle_or_current & 0xFF;         // 角度低字节

    // 8-9. 速度 (位置7-8放速度！)
    uint16_t speed_val = (uint16_t)(speed * 10.0f); // 速度乘以10
    data[7] = (speed_val >> 8) & 0xFF;   // 速度高字节
    data[8] = speed_val & 0xFF;          // 速度低字节

    // 10. 校验 (前9字节异或和)
    uint8_t bcc = 0;
    for (int i = 0; i < 9; ++i) {
        bcc ^= data[i];
    }
    data[9] = bcc;

    // 11. 帧尾
    data[10] = 0x7D;

    printf("Sending data: ");
    for (int i = 0; i < sizeof(data); ++i) {
        printf("%02X ", data[i]);
        if (i == 10) {
            printf("\n");
        }
    }
    
    // 发送数据
    HAL_UART_Transmit(&huart6, data, sizeof(data), HAL_MAX_DELAY);
}


void Stepper_Motor_Init(void)
{
    // 初始化步进电机UART到绝对0点
    Stepper_Motor_Control(0, StepMotor_Mode_Angle,0, 1.0f, 0.0f); // 初始化步进电机0
    Stepper_Motor_Control(1, StepMotor_Mode_Angle,0, 1.0f, 0.0f); // 初始化步进电机1

}



void Stepper_Read_Proc(void)
{
    // 读取步进电机数据
    static uint8_t read_count = 0;
    if (read_count == 0)
    {
        Stepper1_SendRequest(); // 发送请求给步进电机1
        read_count = 1;
    }
    else if (read_count == 1)
    {
        Stepper0_SendRequest(); // 发送请求给步进电机0
        read_count = 0;
        
    }
}



void addx(void) // 正确的
{

    //Stepper_Motor_Control(0, StepMotor_Mode_Position, 0, 1.0f, 0.1f);

    if (StepMotor0.angle <= -90.0f || StepMotor0.angle >= 90.0f)
    {
        //Stepper_Motor_Control(0, StepMotor_Mode_Speed, 0, 0.0f, 0.0f); // 停止步进电机0
        //printf("Stepper Motor 0 stopped due to angle limit.\n");
    }
    else 
    {
        Stepper_Motor_Control(0, StepMotor_Mode_Speed, 0, 0.1f, 0.1f); // 正向移动
    }
}
void deductx(void)
{

    //Stepper_Motor_Control(0, StepMotor_Mode_Position, 1, 1.0f, 0.1f);

    if (StepMotor0.angle <= - 90.0f || StepMotor0.angle >= 90.0f)
    {
        //Stepper_Motor_Control(0, StepMotor_Mode_Speed, 0, 0.0f, 0.0f); // 停止步进电机0
        //printf("Stepper Motor 0 stopped due to angle limit.\n");
    }
    else 
    {
        Stepper_Motor_Control(0, StepMotor_Mode_Speed, 1, 0.1f, 0.1f); // 反向移动
    }
}

void addy(void) // 正确的
{

    // uint8_t data[] = {0x7B, 0x00, 0x02, 0x01, 0x20, 0x00, 0x0A, 0x00, 0x64, 0x36, 0x7D};
    // HAL_UART_Transmit(&huart6, data, sizeof(data), HAL_MAX_DELAY);
    // Stepper_Motor_Control(1, StepMotor_Mode_Position, 0, 1.0f, 0.1f);
    
    if (StepMotor1.angle <= -90.0f || StepMotor1.angle >= 90.0f)
    {
        //Stepper_Motor_Control(1, StepMotor_Mode_Speed, 0, 0.0f, 0.0f); // 停止步进电机1
        //printf("Stepper Motor 1 stopped due to angle limit.\n");
    }
    else 
    {
        Stepper_Motor_Control(1, StepMotor_Mode_Speed, 0, 0.1f, 0.1f); // 正向移动
    }
}
void deducty(void)
{
    // uint8_t data[] =  {0x7B, 0x00, 0x02, 0x00, 0x20, 0x00, 0x0A, 0x00, 0x64, 0x37, 0x7D};
    // HAL_UART_Transmit(&huart6, data, sizeof(data), HAL_MAX_DELAY);
    // Stepper_Motor_Control(1, StepMotor_Mode_Position, 1, 1.0f, 0.1f);
    if (StepMotor1.angle <= -90.0f || StepMotor1.angle >= 90.0f)
    {
        //Stepper_Motor_Control(1, StepMotor_Mode_Speed, 0, 0.0f, 0.0f); // 停止步进电机1
        //printf("Stepper Motor 1 stopped due to angle limit.\n");
    }
    else 
    {
        Stepper_Motor_Control(1, StepMotor_Mode_Speed, 1, 0.1f, 0.1f); // 反向移动
    }
            
}
void Stepper_AllStop(void)
{
    // 停止所有步进电机
    Stepper_Motor_Control(0, StepMotor_Mode_Speed, 0, 0.0f, 0.0f); // 停止步进电机0
    //HAL_Delay(10); // 确保停止命令被处理
    Stepper_Motor_Control(1, StepMotor_Mode_Speed, 0, 0.0f, 0.0f); // 停止步进电机1
}